/*
 * FusionPosition2d.java
 *
 * Created on 14 de febrero de 2008, 07:01 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Robot;
import Utils.*;
import javaclient2.*;
import javaclient2.structures.PlayerConstants;
import javaclient2.structures.PlayerPose;
import java.lang.Exception;

public class FusionPosition2d  extends Position2d{

    
    private Position2DInterface actuator=null;
    private Position2DInterface sensor=null;
    //static int index=0;

    /**
     *Constructor of the class
     */
    public FusionPosition2d(Robot robotSensor, Robot robotActuator, String name, int index){
        
        //constructor of father-class
        super(robotActuator, name, index);
        
        
        
            
        //actuator=robotActuator.getConnection().requestInterfacePosition2D(deviceIndex, PlayerConstants.PLAYER_OPEN_MODE);
        actuator=robotActuator.requestInterfacePosition2D(0, PlayerConstants.PLAYER_OPEN_MODE);

        if(actuator!=null)
            System.out.println("Interface position2d:"+
                                Integer.toString(deviceIndex)+
                                    " of Robot:"+
                                        Integer.toString(robot.getIndex())+" Ok");
        else{
            System.out.println("Interface position2d:"+
                                Integer.toString(deviceIndex)+
                                    " of Robot:"+
                                        Integer.toString(robot.getIndex())+" Fail");
            return;
        }
        this.setPlayerDevice(actuator);

        sensor=robotSensor.requestInterfacePosition2D(deviceIndex, PlayerConstants.PLAYER_OPEN_MODE);

        if(sensor!=null)
            System.out.println("Interface position2d:"+
                                Integer.toString(deviceIndex)+
                                    " of Robot:"+
                                        Integer.toString(robot.getIndex())+" Ok");
        else{
            System.out.println("Interface position2d:"+
                                Integer.toString(deviceIndex)+
                                    " of Robot:"+
                                        Integer.toString(robot.getIndex())+" Fail");
            return;
        }
        //index++;

        
        
        this.setPlayerDevice(sensor);
        
        data=new Pose2d(0f,0f,0f);
        actuator.setMotorPower(1);
        //wait until update was succesfull
        //while(!update());
    }


    /**
     *
     * Define si se puede leer o no
     */
    public boolean isReady()
    {
        return sensor.isDataReady();
    }
    /**
     *
     * Pos inicial
     */
    public void setPosition(Pose2d initial){
        actuator.resetOdometry();
        actuator.setOdometry(initial);
        data.copy(initial);
    }
    
    /**
     *
     * Pos inicial
     */
    public boolean update(){
        
       
        if(isReady()){
            //System.out.println("data ready on Position2d");
            PlayerPose x = sensor.getData().getPos(); 
            float a = (float)x.getPa();
            data.setPa(a);
            data.setPx(x.getPx());
            data.setPy(x.getPy());
            //data.add(initialPosition);
            //System.out.println("robot angle fusion: "+ data.getPa());
            return true;
        }
        else
            return false;
           
    
    }
    
    
     
    public void setSpeed(float val1, float val2){
//        PlayerPose vel=new  PlayerPose();
//        vel.setPx(val1);
//        vel.setPy(0.0f);
//        vel.setPa(val2);
        actuator.setSpeed(val1, val2);
        //position.setVelocity(vel,0);
    }
    

    
}
